// Matrix 4x4
// (c) jimon game studio

#ifndef JEH_JEMATRIX4
#define JEH_JEMATRIX4

#include "jeTypes.h"
#include "jeVector3d.h"

namespace je
{

	// Row Major Matrix Order
	#define JE_MATRIX4X4_ORDER_RULE(i,j) 4*i+j

	//! Matrix 4x4
	//! Enesea Matix 4x4 Format
	//!     j00 j01 j02 j03
	//! i00 e00 e01 e02 e03
	//! i01 e04 e05 e06 e07
	//! i02 e08 e09 e10 e11
	//! i03 e12 e13 e14 e15
	//! matrix row-major, row-major rule is in JE_MATRIX4X4_ORDER_RULE(i,j)
	template <typename T>
	class jeMatrix4
	{
	public:
		T Element[16];

		//! Constructor
		jeMatrix4()
		{
			Identity();
		}

		//! Constructor
		jeMatrix4(const jeMatrix4<T> & Other)
		{
			memcpy(Element,Other.Element,sizeof(T)*16);
		}

		//! Destructor
		~jeMatrix4()
		{
		}

		//! Clear
		inline void Clear()
		{
			memset(Element,0,sizeof(T)*16);
		}

		//! Identity
		void Identity()
		{
			Clear();

			Set(0,0,1);
			Set(1,1,1);
			Set(2,2,1);
			Set(3,3,1);
		}

		//! Return index
		inline u32 GetIndex(u8 i,u8 j) const
		{
			return JE_MATRIX4X4_ORDER_RULE(i,j);
		}

		//! Return element
		inline T Get(u8 i,u8 j) const
		{
			return Element[JE_MATRIX4X4_ORDER_RULE(i,j)];
		}

		//! Set Element
		inline void Set(u8 i,u8 j,T NewElement)
		{
			Element[JE_MATRIX4X4_ORDER_RULE(i,j)] = NewElement;
		}

		//! operator =
		jeMatrix4<T> & operator = (const jeMatrix4<T> & Other)
		{
			memcpy(Element,Other.Element,sizeof(T)*16);
			return *this;
		}

		//! return true if our vector == other vector
		u1 Equal(const jeMatrix4<T> & Other) const
		{
			u1 Equal_Flag = true;
			for(u8 i = 0;i < 16;++i)
				if (Element[i] != Other.Element[i])
				{
					Equal_Flag = false;
					break;
				}

			return Equal_Flag;
		}

		//! operator ==
		u1 operator == (const jeMatrix4<T> & Other) const
		{
			return Equal(Other);
		}

		//! operator !=
		u1 operator != (const jeMatrix4<T> & Other) const
		{
			return !Equal(Other);
		}

		//! operator +
		jeMatrix4<T> operator + (const jeMatrix4<T> & Other) const
		{
			jeMatrix4<T> Copy;
			for(u8 i = 0;i < 16;++i)
				Copy.Element[i] = Element[i] + Other.Element[i];
			return Copy;
		}

		//! operator -
		jeMatrix4<T> operator - (const jeMatrix4<T> & Other) const
		{
			jeMatrix4<T> Copy;
			for(u8 i = 0;i < 16;++i)
				Copy.Element[i] = Element[i] - Other.Element[i];
			return Copy;
		}

		//! Direct Access Operator
		inline T & operator [](u32 Index)
		{
			return Element[Index];
		}

		//! operator *
		#ifdef JE_MSVC
		__forceinline jeMatrix4<T> operator * (const jeMatrix4<T> & Other) const
		#else
		jeMatrix4<T> operator * (const jeMatrix4<T> & Other) const
		#endif
		{
			jeMatrix4<T> Copy;

			#define JE_MULTMATRIX_ELEMENT(i,j) Copy.Element[JE_MATRIX4X4_ORDER_RULE(i,j)] = (*this).Element[JE_MATRIX4X4_ORDER_RULE(i,0)]*Other.Element[JE_MATRIX4X4_ORDER_RULE(0,j)]+(*this).Element[JE_MATRIX4X4_ORDER_RULE(i,1)]*Other.Element[JE_MATRIX4X4_ORDER_RULE(1,j)]+(*this).Element[JE_MATRIX4X4_ORDER_RULE(i,2)]*Other.Element[JE_MATRIX4X4_ORDER_RULE(2,j)]+(*this).Element[JE_MATRIX4X4_ORDER_RULE(i,3)]*Other.Element[JE_MATRIX4X4_ORDER_RULE(3,j)];

			JE_MULTMATRIX_ELEMENT(0,0)
			JE_MULTMATRIX_ELEMENT(0,1)
			JE_MULTMATRIX_ELEMENT(0,2)
			JE_MULTMATRIX_ELEMENT(0,3)
			JE_MULTMATRIX_ELEMENT(1,0)
			JE_MULTMATRIX_ELEMENT(1,1)
			JE_MULTMATRIX_ELEMENT(1,2)
			JE_MULTMATRIX_ELEMENT(1,3)
			JE_MULTMATRIX_ELEMENT(2,0)
			JE_MULTMATRIX_ELEMENT(2,1)
			JE_MULTMATRIX_ELEMENT(2,2)
			JE_MULTMATRIX_ELEMENT(2,3)
			JE_MULTMATRIX_ELEMENT(3,0)
			JE_MULTMATRIX_ELEMENT(3,1)
			JE_MULTMATRIX_ELEMENT(3,2)
			JE_MULTMATRIX_ELEMENT(3,3)

			return Copy;
		}

		//! operator *
		#ifdef JE_MSVC
		__forceinline jeMatrix4<T> operator * (const T & Other) const
		#else
		jeMatrix4<T> operator * (const T & Other) const
		#endif
		{
			jeMatrix4<T> Copy;

			for(u32 i = 0;i < 16;i++)
				Copy.Element[i] = Element[i] * Other;

			return Copy;
		}

		//! Rotate Vector
		inline jeVector3d<T> RotateVector(const jeVector3d<T> & Vector) const
		{
			jeVector3d<T> Temp;

			Temp.X = Get(0,0) * Vector.X + Get(1,0) * Vector.Y + Get(2,0) * Vector.Z;
			Temp.Y = Get(0,1) * Vector.X + Get(1,1) * Vector.Y + Get(2,1) * Vector.Z;
			Temp.Z = Get(0,2) * Vector.X + Get(1,2) * Vector.Y + Get(2,2) * Vector.Z;

			return Temp;
		}

		//! Multiply Vector On Matrix
		inline jeVector3d<T> MultiplyVectorOnMatrix(const jeVector3d<T> & Vector) const
		{
			jeVector3d<T> Temp;

			Temp.X = Get(0,0) * Vector.X + Get(1,0) * Vector.Y + Get(2,0) * Vector.Z + Get(3,0) * 1;
			Temp.Y = Get(0,1) * Vector.X + Get(1,1) * Vector.Y + Get(2,1) * Vector.Z + Get(3,1) * 1;
			Temp.Z = Get(0,2) * Vector.X + Get(1,2) * Vector.Y + Get(2,2) * Vector.Z + Get(3,2) * 1;

			return Temp;
		}

		//! Multiply Matrix On Vector
		inline jeVector3d<T> MultiplyMatrixOnVector(const jeVector3d<T> & Vector) const
		{
			jeVector3d<T> Temp;

			Temp.X = Get(0,0) * Vector.X + Get(0,1) * Vector.Y + Get(0,2) * Vector.Z + Get(0,3) * 1;
			Temp.Y = Get(1,0) * Vector.X + Get(1,1) * Vector.Y + Get(1,2) * Vector.Z + Get(1,3) * 1;
			Temp.Z = Get(2,0) * Vector.X + Get(2,1) * Vector.Y + Get(2,2) * Vector.Z + Get(2,3) * 1;

			return Temp;
		}

		//! Transpose
		void Transpose()
		{
			jeMatrix4<T> Temp;

			for(u32 i=0;i<4;i++)
				for(u32 j=0;j<4;j++)
					Temp.Set(j,i,Get(i,j));

			(*this) = Temp;
		}

		//! Build Perspective Matrix
		void BuildPerspectiveMatrix(f32 Fov,f32 Aspect,f32 Near,f32 Far)
		{
			T W = 1.0f/tan(Fov * jeDegToRad / 2.0f);

			Clear();

			Set(0,0,W/Aspect);
			Set(1,1,W);
			Set(2,2,(Near+Far)/(Near-Far));
			Set(2,3,(2.0f*Near*Far)/(Near-Far));
			Set(3,2,-1);
		}

		//! Build Orto Matrix
		void BuildOrtoMatrix(f32 Left,f32 Right,f32 Bottom,f32 Top,f32 Near,f32 Far)
		{
			Clear();
			Set(0,0,2.0f/(Right-Left));
			Set(1,1,2.0f/(Top-Bottom));
			Set(2,2,-2.0f/(Far-Near));
			Set(0,3,-(Right+Left)/(Right-Left));
			Set(1,3,-(Top+Bottom)/(Top-Bottom));
			Set(2,3,-(Far+Near)/(Far-Near));
			Set(3,3,1);
		}

		//! Build Look At Matrix
		void BuildLookAtMatrix(const jeVector3d<T> & Eye,const jeVector3d<T> & LookAt,const jeVector3d<T> & UpVector)
		{
			jeVector3d<T> ZAxis = Eye - LookAt;
			ZAxis.Normalize();

			jeVector3d<T> XAxis = UpVector.CrossProduct(ZAxis);
			XAxis.Normalize();

			jeVector3d<T> YAxis = ZAxis.CrossProduct(XAxis);
			YAxis.Normalize();

			Clear();

			Set(0,0,XAxis.X);
			Set(0,1,XAxis.Y);
			Set(0,2,XAxis.Z);
			Set(0,3,-XAxis.DotProduct(Eye));
			Set(1,0,YAxis.X);
			Set(1,1,YAxis.Y);
			Set(1,2,YAxis.Z);
			Set(1,3,-YAxis.DotProduct(Eye));
			Set(2,0,ZAxis.X);
			Set(2,1,ZAxis.Y);
			Set(2,2,ZAxis.Z);
			Set(2,3,-ZAxis.DotProduct(Eye));
			Set(3,3,1);
		}

		//! Translate
		void Translate(const jeVector3df & Position)
		{
			if(!Position.GetDistanceQR())
				return;

			jeMatrix4<T> Mat;
			Mat.Set(0,3,Position.X);
			Mat.Set(1,3,Position.Y);
			Mat.Set(2,3,Position.Z);

			(*this) = Mat * (*this);
		}

		//! Get Translation From Matrix
		jeVector3df GetTranslation() const
		{
			jeVector3df Translation;

			Translation.X = Get(0,3);
			Translation.Y = Get(1,3);
			Translation.Z = Get(2,3);

			return Translation;
		}
		
		//! Clear Translation (set translation to 0,0,0)
		void ClearTranslation()
		{
			Set(0,3,0);
			Set(1,3,0);
			Set(2,3,0);
		}

		//! Rotate
		void Rotate(const jeVector3df & Rotation)
		{
			if(!Rotation.GetDistanceQR())
				return;

			jeMatrix4<T> Mat;

			T Sr,Cr,Sp,Cp,Sy,Cy,SrSp,CrSp;
			jeSinCos(Rotation.X*jeDegToRad,Sr,Cr);
			jeSinCos(Rotation.Y*jeDegToRad,Sp,Cp);
			jeSinCos(Rotation.Z*jeDegToRad,Sy,Cy);
			SrSp = Sr*Sp;
			CrSp = Cr*Sp;

			Mat.Set(0,0,Cp*Cy);
			Mat.Set(0,1,Cp*Sy);
			Mat.Set(0,2,-Sp);
			Mat.Set(1,0,SrSp*Cy-Cr*Sy);
			Mat.Set(1,1,SrSp*Sy+Cr*Cy);
			Mat.Set(1,2,Sr*Cp);
			Mat.Set(2,0,CrSp*Cy+Sr*Sy);
			Mat.Set(2,1,CrSp*Sy-Sr*Cy);
			Mat.Set(2,2,Cr*Cp);

			(*this) = Mat * (*this);
		}

		//! Scale
		void Scale(const jeVector3df & Scale)
		{
			if(!Scale.GetDistanceQR())
				return;

			jeMatrix4<T> Mat;
			Mat.Set(0,0,Scale.X);
			Mat.Set(1,1,Scale.Y);
			Mat.Set(2,2,Scale.Z);

			(*this) = Mat * (*this);
		}

		//! Get Normal Matrix (it's 3x3, inverted, transposed)
		inline jeMatrix4<T> GetNormalMatrix() const
		{
			// this code like shit :) but it's fast because SSE
			jeMatrix4<T> NMat;

			f32 Det = 0.0f;
			#define DETMATRIX(x1,y1,x2,y2,x3,y3,x4,y4,x5,y5,x6,y6) Det+=Element[JE_MATRIX4X4_ORDER_RULE(x1,y1)]*Element[JE_MATRIX4X4_ORDER_RULE(x2,y2)]*Element[JE_MATRIX4X4_ORDER_RULE(x3,y3)]-Element[JE_MATRIX4X4_ORDER_RULE(x4,y4)]*Element[JE_MATRIX4X4_ORDER_RULE(x5,y5)]*Element[JE_MATRIX4X4_ORDER_RULE(x6,y6)];
			DETMATRIX(0,0,1,1,2,2,0,0,1,2,2,1)
			DETMATRIX(1,0,0,2,2,1,1,0,0,1,2,2)
			DETMATRIX(2,0,0,1,1,2,2,0,0,2,1,1)

			if(Det == 0.0f)
				return NMat;

			Det = 1.0f / Det;

			#define FILLMATRIX(x1,y1,x2,y2,x3,y3,x4,y4,x5,y5) NMat.Element[JE_MATRIX4X4_ORDER_RULE(x1,y1)] = (Element[JE_MATRIX4X4_ORDER_RULE(x2,y2)] * Element[JE_MATRIX4X4_ORDER_RULE(x3,y3)] - Element[JE_MATRIX4X4_ORDER_RULE(x4,y4)] * Element[JE_MATRIX4X4_ORDER_RULE(x5,y5)]) * Det;
			FILLMATRIX(0,0,1,1,2,2,1,2,2,1)
			FILLMATRIX(0,1,1,2,2,0,1,0,2,2)
			FILLMATRIX(0,2,1,0,2,1,1,1,2,0)
			FILLMATRIX(1,0,0,2,2,1,0,1,2,2)
			FILLMATRIX(1,1,0,0,2,2,0,2,2,0)
			FILLMATRIX(1,2,0,1,2,0,0,0,2,1)
			FILLMATRIX(2,0,0,1,1,2,0,2,1,1)
			FILLMATRIX(2,1,0,2,1,0,0,0,1,2)
			FILLMATRIX(2,2,0,0,1,1,0,1,1,0)

			return NMat;
		}

		//! Get Inverse
		inline u1 GetInverse(jeMatrix4<T> & Out,u1 IgnoreDetCheck = false) const
		{
			f32 Det = (Get(0,0)*Get(1,1)-Get(0,1)*Get(1,0))*(Get(2,2)*Get(3,3)-Get(2,3)*Get(3,2))-(Get(0,0)*Get(1,2)-Get(0,2)*Get(1,0))*(Get(2,1)*Get(3,3)-Get(2,3)*Get(3,1))+(Get(0,0)*Get(1,3)-Get(0,3)*Get(1,0))*(Get(2,1)*Get(3,2)-Get(2,2)*Get(3,1))+(Get(0,1)*Get(1,2)-Get(0,2)*Get(1,1))*(Get(2,0)*Get(3,3)-Get(2,3)*Get(3,0))-(Get(0,1)*Get(1,3)-Get(0,3)*Get(1,1))*(Get(2,0)*Get(3,2)-Get(2,2)*Get(3,0))+(Get(0,2)*Get(1,3)-Get(0,3)*Get(1,2))*(Get(2,0)*Get(3,1)-Get(2,1)*Get(3,0));

			if((!IgnoreDetCheck) && (fabsf(Det) < jeRoundingError32))
				return false;

			Det = 1.0f / Det;

			Out.Set(0,0,Det*(Get(1,1)*(Get(2,2)*Get(3,3)-Get(2,3)*Get(3,2))+Get(1,2)*(Get(2,3)*Get(3,1)-Get(2,1)*Get(3,3))+Get(1,3)*(Get(2,1)*Get(3,2)-Get(2,2)*Get(3,1))));
			Out.Set(0,1,Det*(Get(2,1)*(Get(0,2)*Get(3,3)-Get(0,3)*Get(3,2))+Get(2,2)*(Get(0,3)*Get(3,1)-Get(0,1)*Get(3,3))+Get(2,3)*(Get(0,1)*Get(3,2)-Get(0,2)*Get(3,1))));
			Out.Set(0,2,Det*(Get(3,1)*(Get(0,2)*Get(1,3)-Get(0,3)*Get(1,2))+Get(3,2)*(Get(0,3)*Get(1,1)-Get(0,1)*Get(1,3))+Get(3,3)*(Get(0,1)*Get(1,2)-Get(0,2)*Get(1,1))));
			Out.Set(0,3,Det*(Get(0,1)*(Get(1,3)*Get(2,2)-Get(1,2)*Get(2,3))+Get(0,2)*(Get(1,1)*Get(2,3)-Get(1,3)*Get(2,1))+Get(0,3)*(Get(1,2)*Get(2,1)-Get(1,1)*Get(2,2))));
			Out.Set(1,0,Det*(Get(1,2)*(Get(2,0)*Get(3,3)-Get(2,3)*Get(3,0))+Get(1,3)*(Get(2,2)*Get(3,0)-Get(2,0)*Get(3,2))+Get(1,0)*(Get(2,3)*Get(3,2)-Get(2,2)*Get(3,3))));
			Out.Set(1,1,Det*(Get(2,2)*(Get(0,0)*Get(3,3)-Get(0,3)*Get(3,0))+Get(2,3)*(Get(0,2)*Get(3,0)-Get(0,0)*Get(3,2))+Get(2,0)*(Get(0,3)*Get(3,2)-Get(0,2)*Get(3,3))));
			Out.Set(1,2,Det*(Get(3,2)*(Get(0,0)*Get(1,3)-Get(0,3)*Get(1,0))+Get(3,3)*(Get(0,2)*Get(1,0)-Get(0,0)*Get(1,2))+Get(3,0)*(Get(0,3)*Get(1,2)-Get(0,2)*Get(1,3))));
			Out.Set(1,3,Det*(Get(0,2)*(Get(1,3)*Get(2,0)-Get(1,0)*Get(2,3))+Get(0,3)*(Get(1,0)*Get(2,2)-Get(1,2)*Get(2,0))+Get(0,0)*(Get(1,2)*Get(2,3)-Get(1,3)*Get(2,2))));
			Out.Set(2,0,Det*(Get(1,3)*(Get(2,0)*Get(3,1)-Get(2,1)*Get(3,0))+Get(1,0)*(Get(2,1)*Get(3,3)-Get(2,3)*Get(3,1))+Get(1,1)*(Get(2,3)*Get(3,0)-Get(2,0)*Get(3,3))));
			Out.Set(2,1,Det*(Get(2,3)*(Get(0,0)*Get(3,1)-Get(0,1)*Get(3,0))+Get(2,0)*(Get(0,1)*Get(3,3)-Get(0,3)*Get(3,1))+Get(2,1)*(Get(0,3)*Get(3,0)-Get(0,0)*Get(3,3))));
			Out.Set(2,2,Det*(Get(3,3)*(Get(0,0)*Get(1,1)-Get(0,1)*Get(1,0))+Get(3,0)*(Get(0,1)*Get(1,3)-Get(0,3)*Get(1,1))+Get(3,1)*(Get(0,3)*Get(1,0)-Get(0,0)*Get(1,3))));
			Out.Set(2,3,Det*(Get(0,3)*(Get(1,1)*Get(2,0)-Get(1,0)*Get(2,1))+Get(0,0)*(Get(1,3)*Get(2,1)-Get(1,1)*Get(2,3))+Get(0,1)*(Get(1,0)*Get(2,3)-Get(1,3)*Get(2,0))));
			Out.Set(3,0,Det*(Get(1,0)*(Get(2,2)*Get(3,1)-Get(2,1)*Get(3,2))+Get(1,1)*(Get(2,0)*Get(3,2)-Get(2,2)*Get(3,0))+Get(1,2)*(Get(2,1)*Get(3,0)-Get(2,0)*Get(3,1))));
			Out.Set(3,1,Det*(Get(2,0)*(Get(0,2)*Get(3,1)-Get(0,1)*Get(3,2))+Get(2,1)*(Get(0,0)*Get(3,2)-Get(0,2)*Get(3,0))+Get(2,2)*(Get(0,1)*Get(3,0)-Get(0,0)*Get(3,1))));
			Out.Set(3,2,Det*(Get(3,0)*(Get(0,2)*Get(1,1)-Get(0,1)*Get(1,2))+Get(3,1)*(Get(0,0)*Get(1,2)-Get(0,2)*Get(1,0))+Get(3,2)*(Get(0,1)*Get(1,0)-Get(0,0)*Get(1,1))));
			Out.Set(3,3,Det*(Get(0,0)*(Get(1,1)*Get(2,2)-Get(1,2)*Get(2,1))+Get(0,1)*(Get(1,2)*Get(2,0)-Get(1,0)*Get(2,2))+Get(0,2)*(Get(1,0)*Get(2,1)-Get(1,1)*Get(2,0))));

			return true;
		}

		//! Is Identity
		inline u1 IsIdentity() const
		{
			for(u8 i=0;i<4;i++)
				for(u8 j=0;j<4;j++)
					if(Get(i,j)!=(i==j?1.0f:0.0f))
						return false;
			return true;
		}
	};

	//! f32 Matrix 4x4
	typedef jeMatrix4<f32> jeMatrix4f;
};

#endif
